#include "physics.h"
#include "characteractor.h"
#include "..\common\def.h"
#include "..\common\util.h"
#include "..\engine\error.h"
#include "..\engine\engine.h"
#include "..\engine\time.h"

#include "Physics\Dynamics\Entity\hkpRigidBodyCinfo.h"
#include "Physics\Collide\Shape\Compound\Collection\hkpShapeCollection.h"
#include "common\base\math\vector\hkVector4.h"
#include "Common\Base\Types\hkBaseTypes.h"

#include "Common\Base\Types\Physics\hkStepInfo.h"
#include "Common\Base\Math\Matrix\hkTransform.h"

// Used for
// character rigid body
#include <Physics/Utilities/CharacterControl/CharacterRigidBody/hkpCharacterRigidBody.h>
// character proxy
#include <Physics/Utilities/CharacterControl/CharacterProxy/hkpCharacterProxy.h>
// state machine
/**************************************
 *
 **************************************/
CCharacterActor::CCharacterActor(CPhysics* const pPhysics) : CActor(pPhysics)
{	
	pGroundState = NULL;
	pAirState = NULL;
	pJumpState = NULL;

	m_characterContext = NULL;
	m_characterRigidBody = NULL;
}

/**************************************
 *
 **************************************/
CCharacterActor::~CCharacterActor()
{
	Release();
}

/**************************************
 *Enables the client to determine the
 *state of the character
 **************************************/
CCharacterActor::CharacterState CCharacterActor::GetState()
{
	switch( m_characterContext->getState() )
	{
		case HK_CHARACTER_ON_GROUND:
			return CharacterState::ON_GROUND;
		case HK_CHARACTER_IN_AIR:
			return CharacterState::IN_AIR;
		case HK_CHARACTER_JUMPING:
			return CharacterState::JUMPING;
	}

	return CharacterState::OTHER;
}

/**************************************
 *Enables the client to specify a movement
 *vector for this frame
 **************************************/
void CCharacterActor::ApplyMovement(const float walkSpeed,const float sidestepSpeed,  const Vector3f& forwardDirection, const bool jump)
{
	CEngine::GetInstance()->GetPhysics()->LockSimulation();
	hkpCharacterInput input;
	hkpCharacterOutput output;
	
	input.m_inputLR = sidestepSpeed;
	input.m_inputUD = walkSpeed;

	input.m_wantJump =  jump;
	input.m_atLadder = false;
	
	input.m_up = hkVector4(0,1,0);
	input.m_forward = TohkVector4(forwardDirection);

	float m_timestep = CEngine::GetInstance()->GetTime()->GetTimeDelta();
	if(m_timestep==0)
		m_timestep=0.1f;

	hkStepInfo stepInfo;
	stepInfo.m_deltaTime = m_timestep;
	stepInfo.m_invDeltaTime = 1.0f/m_timestep;
	stepInfo.m_endTime = CEngine::GetInstance()->GetTimeDuration();

	input.m_stepInfo = stepInfo;
	input.m_characterGravity.set(0,-9.8 * 2,0);
	input.m_velocity = m_characterRigidBody->getLinearVelocity();
	input.m_position = m_characterRigidBody->getPosition();

	hkpSurfaceInfo ground;
	m_characterRigidBody->checkSupport(stepInfo,ground);

	input.m_isSupported = (ground.m_supportedState == hkpSurfaceInfo::SUPPORTED);
	input.m_surfaceNormal = ground.m_surfaceNormal;
	input.m_surfaceVelocity = ground.m_surfaceVelocity;
	input.m_surfaceMotionType = ground.m_surfaceMotionType;

	m_characterContext->update(input, output);

 	m_characterRigidBody->setLinearVelocity(output.m_velocity, m_timestep);
	CEngine::GetInstance()->GetPhysics()->UnlockSimulation();
}

/**************************************
 *Enables the client to specify a movement
 *vector for this frame
 **************************************/
void CCharacterActor::ApplyMovement(const Vector3f& deltaMovement)
{
		
	CEngine::GetInstance()->GetPhysics()->LockSimulation();
	hkpCharacterInput input;
	hkpCharacterOutput output;
	
	input.m_inputLR = deltaMovement.x;
	input.m_inputUD = deltaMovement.z;

	input.m_wantJump =  (deltaMovement.y > 0.0f);
	input.m_atLadder = false;

	
	input.m_up = hkVector4(0,1,0);
	input.m_forward.set(1,0,0);

	float m_timestep = CEngine::GetInstance()->GetTime()->GetTimeDelta() * 2;
	if(m_timestep==0)
		m_timestep=0.1f;

	hkStepInfo stepInfo;
	stepInfo.m_deltaTime = m_timestep;
	stepInfo.m_invDeltaTime = 1.0f/m_timestep;
	stepInfo.m_endTime = CEngine::GetInstance()->GetTimeDuration();

	input.m_stepInfo = stepInfo;
	input.m_characterGravity.set(0,-9.8 * 2,0);
	input.m_velocity = m_characterRigidBody->getLinearVelocity();
	input.m_position = m_characterRigidBody->getPosition();

	hkpSurfaceInfo ground;
	m_characterRigidBody->checkSupport(stepInfo,ground);

	input.m_isSupported = (ground.m_supportedState == hkpSurfaceInfo::SUPPORTED);
	input.m_surfaceNormal = ground.m_surfaceNormal;
	input.m_surfaceVelocity = ground.m_surfaceVelocity;
	input.m_surfaceMotionType = ground.m_surfaceMotionType;
		
	
	m_characterContext->update(input, output);

 	m_characterRigidBody->setLinearVelocity(output.m_velocity, m_timestep);
	CEngine::GetInstance()->GetPhysics()->UnlockSimulation();

}

/**************************************
 *Allocates physics resources which are
 *defined by data within the specified file
 *for use in the application
 **************************************/
HRESULT CCharacterActor::Create(const string& filename, CCharacterActor::ActorData actorData,CObject *pParent)
{
	FILE* pFile = NULL;
	short header;
	short footer;

	try {

		if(!filename.IsValid())
			throw EE_EXCEPTION("Invalid filename.");

		pFile = CEngine::GetInstance()->FindFile(filename);
		if(ISNULL(pFile))
			throw EE_EXCEPTION("Failed to locate or open file.");

		fread(&header, sizeof(short), 1, pFile);
		if(header != 200)
			throw EE_EXCEPTION("Invalid or corrupt file.");
				
		m_pPhysics->LockSimulation();

		hkpCharacterRigidBodyCinfo info;
		info.m_mass = actorData.mass;
		info.m_maxForce = actorData.maxForce;
		info.m_up = hkVector4(0.0f,1.0f,0.0f);
		info.m_maxSlope = actorData.maxSlope;
		//info.m_allowedPenetrationDepth = 0.5f;
		//info.m_supportDistance = 0.5f;
		info.m_friction = actorData.friction;
		//info.m_maxSpeedForSimplexSolver = actorData.maxLinearSpeed;

		if(FAILED(CreateShape(filename, &info.m_shape)))
			throw EE_EXCEPTION("Failed to create shape.");

		m_characterRigidBody = new hkpCharacterRigidBody( info );
		if(ISNULL(m_characterRigidBody))
			throw EE_EXCEPTION("Failed to create ridgid character body.");

		
		m_pActor = m_characterRigidBody->getRigidBody();
		m_pActor->setUserData((hkUlong)pParent);
		m_pPhysics->GetScene()->addEntity( m_pActor);
		m_pActor->setTransform(TohkTransform(actorData.initalTransform));
		m_pActor->setQualityType(HK_COLLIDABLE_QUALITY_CRITICAL);

		
		hkpCharacterStateManager* manager = new hkpCharacterStateManager();

		pGroundState = new hkpCharacterStateOnGround();
		pGroundState->setSpeed(10);
		pGroundState->setDisableHorizontalProjection(false);
		pGroundState->setGroundHugging(true);

		manager->registerState( pGroundState,	HK_CHARACTER_ON_GROUND);
		pGroundState->removeReference();

		pAirState = new hkpCharacterStateInAir();
		manager->registerState(pAirState,	HK_CHARACTER_IN_AIR);
		pAirState->removeReference();

		pJumpState = new hkpCharacterStateJumping();
		pJumpState->setJumpHeight(5.05f);
		manager->registerState( pJumpState,	HK_CHARACTER_JUMPING);
		pJumpState->removeReference();

		//state = new hkpCharacterStateClimbing();
		//manager->registerState( state,	HK_CHARACTER_CLIMBING);
		//state->removeReference();

		m_characterContext = new hkpCharacterContext(manager, HK_CHARACTER_ON_GROUND);
		manager->removeReference();

		m_characterContext->setCharacterType(hkpCharacterContext::HK_CHARACTER_RIGIDBODY);
		
		m_pPhysics->UnlockSimulation();

		return S_OK;

	}catch(CError::CErrorData error)
	{
		if(pFile)
			fclose(pFile);

		Release();

		m_pPhysics->UnlockSimulation();

		CError::CError(error);
		return E_FAIL;
	}

	return S_OK;
}
